
;Original code by Rolf R Bakke 2011, 2012, 2013
;Modified by zerush 2013

;best viewed with a TAB-setting of 8 and monospace font.



.include "m324Pdef.inc"
.include "macros.inc"
.include "miscmacros.inc"
.include "variables.asm"
.include "hardware.asm"
.include "168mathlib_macros.inc"
.include "824mathlib_macros.inc"
.include "constants.asm"

.org 0x0000

	jmp reset
	jmp IntPitch
	jmp IntRoll
	jmp IntYaw
	jmp unused
	jmp IntAux
	jmp unused
	jmp IntThrottle
	jmp unused
	jmp IsrFlightTime
	jmp unused
	jmp unused
	jmp unused
	jmp IsrPwmStart
	jmp IsrPwmEnd
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp IsrSumDReceive
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp unused
	jmp IsrHottReceive
	jmp unused
	jmp unused

unused:	reti


;--- Hardware Init ---

reset:	ldi t,low(ramend)	;initalize stack pointer
	out spl,t
	ldi t,high(ramend)
	out sph,t

	ldx 100
	call WaitXms

	call SetupHardware

	clr t
	sts FlightTimeCentis, t
	sts FlightTimeSeconds, t
	sts FlightTimeMinutes, t

	;---

	call LcdUpdate

	call ShowVersion

;--- Variables init ---

	call EeInit

	lrv MainMenuCursorYposSave, 0
	lrv MainMenuListYposSave, 0
	
	lrv LoadMenuCursorYposSave, 0
	lrv LoadMenuListYposSave, 0
	
	rvsetflagfalse flagStickCommandSelfLevelOn

	lrv Status, 0
	lrv StatusOldState, 0 

	clr t
	sts RollL, t
	sts RollH, t
	sts PitchL, t
	sts PitchH, t
	sts ThrottleL, t
	sts ThrottleH, t
	sts YawL, t
	sts YawH, t
	sts AuxL, t
	sts AuxH, t

	sts CppmChannel1L, t
	sts CppmChannel1H, t
	sts CppmChannel2L, t
	sts CppmChannel2H, t
	sts CppmChannel3L, t
	sts CppmChannel3H, t
	sts CppmChannel4L, t
	sts CppmChannel4H, t
	sts CppmChannel5L, t
	sts CppmChannel5H, t

	b16ldi CheckRxDelay, 400 * 10

	ldz CppmChannel1L
	sts CppmPulseArrayAddressL, zl
	sts CppmPulseArrayAddressH, zh

	call gyrocal

	call FlightInit


;--- MAIN ----
	
	sei


;--- Throttle cal ----

	call GetButtons
	cpi t, 0x09			;both buttons 1 and 4 pressed?
	brne ma5
	call EscThrottleCalibration	;Yes
ma5:


;--- Flight loop init

ma2:	call FlightInit

	rvbrflagfalse flagCppmOn, ma10
	;SUM enabled, disable ISRs for all other RX channels
	ldi t,0b00000000
	store eimsk, t
	rvbrflagtrue flagSumMode, ma11
	;CPPM mode, keep CPPM ISR enabled
	ldi t,0b00001000
	store pcicr, t
	rjmp ma13
ma11:
	;SUMD mode, disable CPPM ISR
	ldi t,0b00000000
	store pcicr, t
	rjmp ma13
ma10:
	;SUM disabled, enable ISRs for all RX channels
	;       76543210
	ldi t,0b00000111
	store eimsk, t
	;       76543210
	ldi t,0b00001010
	store pcicr, t
ma13:

	rvbrflagfalse flagHottOn, ma9
	rvbrflagfalse flagCppmOn, ma9
	;Hott telemetry enabled (and SUM mode), initialize Hott
	call HottInit
ma9:

	rvbrflagfalse flagSumMode, ma12
	rvbrflagfalse flagCppmOn, ma12
	;SUM enabled and SUMD mode
	call SumDInit
ma12:

	;       76543210		;clear pending OCR1A and B interrupt
	ldi t,0b00000110
	store tifr1, t

;--- Flight Loop

ma1:	

	;sbi OutputPin8		;OBS DEBUG

	call PwmStart			;runtime between PwmStart and B interrupt (in PwmEnd) must not exeed 1.5ms
	call GetRxChannels
	call CheckRx
	call Arming
	call Logic
	call Imu
	call HeightDampening
	call Mixer
	call CameraStab
	call Beeper
	call Lva
	call HottUpdate
	call HottPoll
	call PwmEnd

	
	rvcp Status, StatusOldState			;Set LcdUpdate if Status changes and not armed
	breq ma8
	rvmov StatusOldState, Status
	rvflagnot flagA, flagArmed 
	rvflagor  flagLcdUpdate, flagLcdUpdate, flagA 
ma8:
	rvflageor flagA, flagAuxOn, flagAuxOnOldState	;set LcdUpdate true if AuxOn changes state and it is not armed
	rvflagnot flagB, flagArmed
	rvflagand flagA, flagA, flagB 
	rvflagor  flagLcdUpdate, flagLcdUpdate, flagA 
	rvflagand flagAuxOnOldState, flagAuxOn, flagAuxOn

	rvbrflagfalse flagLcdUpdate, ma3		;Update LCD once if flagLcdUpdate is true 
	rvsetflagfalse flagLcdUpdate
	call UpdateFlightDisplay

ma3:	rvbrflagfalse flagArmed, ma7	;skip buttonreading if armed
	rjmp ma1

ma7:	load t, pinb			;read buttons
	com t
	swap t
	andi t, 0x0f
	cpi t, 0x01			;MENY button pressed?
	breq ma4
	
	lrv ButtonDelay, 0		;No, reset ButtonDelay, and go to start of the loop
	rjmp ma1	

ma4:	rvinc ButtonDelay		;yes, ButtonDelay++
	rvcpi ButtonDelay, 50		;ButtonDelay == 50?
	brne ma6			;yes, goto the meny
	rjmp ma1			;no, go to start of the loop	

ma6:

;--- Meny 

;	         76543210		;disable OCR1A and B interrupt
	ldi tt,0b00000000
	store timsk1, tt

	call Beep

	call MainMenu

	rjmp ma2


IsrFlightTime:
	in SregSaver, sreg

	lds tt, FlightTimeCentis
	inc tt
	sts FlightTimeCentis, tt
	cpi tt, 100
	brne IsrFlightTimeEnd
	;a full second has passed
	clr tt
	sts FlightTimeCentis, tt
	lds tt, FlightTimeSeconds
	inc tt
	sts FlightTimeSeconds, tt
	cpi tt, 60
	brne IsrFlightTimeEnd
	;a minute passed
	clr tt
	sts FlightTimeSeconds, tt
	lds tt, FlightTimeMinutes
	inc tt
	sts FlightTimeMinutes, tt

IsrFlightTimeEnd:
	out sreg, SregSaver
	reti

.include "camstab.asm"
.include "trigonometry.asm"
.include "cppmsettings.asm"
.include "checkrx.asm"
.include "setuphw.asm"
.include "version.asm"
.include "reset.asm"
.include "beeper.asm"
.include "menu.asm"
.include "lva.asm"
.include "logic.asm"
.include "heightdamp.asm"
;.include "loader.asm"
.include "selflevel.asm"
.include "layout.asm"
.include "throttlecal.asm"
.include "eeinit.asm"
.include "sensorcal.asm"
.include "settingsc.asm"
.include "settingsb.asm"
.include "settingsa.asm"
.include "flightdisplay.asm"
.include "arm.asm"
.include "flightinit.asm"
.include "debug.asm"
.include "pieditor.asm"
.include "numedit.asm"
.include "mixedit.asm"
.include "mixer2.asm"
.include "imu.asm"
.include "pwmgen.asm"
.include "rxtest.asm"
.include "readrx.asm"
.include "mainmenu.asm"
.include "sensortest.asm"
.include "sensorreading.asm"
.include "ST7565.asm"
.include "miscsubs.asm"
.include "hott.asm"
.include "168mathlib_subs.asm"
.include "824mathlib_subs.asm"
font6x8:
.include "font6x8.asm"
font8x12:
;.include "font8x12.asm"
font12x16:
.include "font12x16.asm"
symbols16x16:
.include "symbols16x16.asm"
font4x6:
.include "font4x6.asm"

builddate:
	.db BUILD_TIMESTAMP ; must be of format: 0xYY,0xYY,0xMM,0xDD


